//#define __FORCE_TRACE__
//#define __FORCE_DEBUG__
//#define __FORCE_INFO__
#define THIS_FILE "ia\fn_patrol.sqf"
#include <tmf_constants.h>
#include <tmf_macros.h>

TRACE_1("Start",_this);
TMFPROFILERSTART;

private["_pos","_max_dist","_grp","_is_vec","_slope_dist","_wp","_grpNumber","_mode","_formation"];
DEFAULT_PARAM(_grp,0,grpNull);
DEFAULT_PARAM(_pos,1,[]);
DEFAULT_PARAM(_max_dist,2,200);
DEFAULT_PARAM(_debug,3,false);
DEFAULT_PARAM(_blacklist,4,[]);

if (AND_ALSO((!isnull _grp),IS_OBJECT(_grp))) then {
	_grp = group _grp;
};
if (!(isnull _grp) && {IS_GROUP(_grp)}) then {
	if (!isServer && !(local (leader _grp))) exitWith { ERROR_1("Execution should be on server or local to the unit",_this); };
	if (IS_STRING(_pos) && {IS_MARKER(_pos)}) then {
		_pos = markerPos _pos;
	};
	if (!AND_ALSO(IS_ARRAY(_pos),(count _pos == 3))) then {
		_pos = getPos (leader _grp);
	};
	
	if (isNil QGVAR(TASK_PATROL_INCREMENT)) then {GVAR(TASK_PATROL_INCREMENT) = 0;};
	GVAR(TASK_PATROL_INCREMENT) = GVAR(TASK_PATROL_INCREMENT) + 1;
	_grpNumber = GVAR(TASK_PATROL_INCREMENT);

	_is_vec = false;
	// We add handlers which will impact the behavior of other units in the group.
	// If a member got killed, it will alert the others. If someone fires near them, it will alert the others.
	// Also, we check if there is at least one unit in a land vehicle (and it's not a static).
	{ 
		_x AddEventHandler ["firedNear", {private ["_grp"]; _grp = group (_this select 0); _grp setspeedmode "FULL"; _grp setCombatMode "RED"; _grp setBehaviour "COMBAT";}];
		_x AddEventHandler ["handleDamage", {private ["_grp"]; _grp = group (_this select 0); _grp setspeedmode "FULL"; _grp setCombatMode "RED"; _grp setBehaviour "COMBAT"; _this select 2;}];
		if (vehicle _x != _x && ((vehicle _x) isKindOf "LandVehicle" && !(vehicle _x isKindOf "StaticWeapon"))) then { _is_vec = true; };
	} foreach units _grp;

	_mode = ["YELLOW", "RED"] call FFUNC(arrayRandomVal);
	_formation = ["STAG COLUMN", "WEDGE", "ECH LEFT", "ECH RIGHT", "VEE", "DIAMOND"] call FFUNC(arrayRandomVal);
	if (_is_vec) then {
		_formation = "LINE";
	};

	DEBUG_1("Beginning",ARR_7(_grpNumber,_grp,_pos,_max_dist,_mode,_formation,count (units _grp)));
	if ((count _pos != 3) || {(([0,0,0] vectorDistance _pos) < 1)} || {count (units _grp) <= 0}) exitWith {
		ERROR("Position not set or empty group");
	};

	_grp setBehaviour "AWARE";
	_grp setSpeedMode "LIMITED";
	_grp setCombatMode _mode;
	_grp setFormation _formation;

	_center_x = (_pos) select 0;
	_center_y = (_pos) select 1;
	_center_z = (_pos) select 2;
	if(isNil "_center_z")then{_center_z = 0;};

	_wp_count = 4 + (floor random 3) + (floor (_max_dist / 100 ));
	_angle = (360 / (_wp_count -1));

	_new_angle = 0;
	_wp_array = [];
	_slack = _max_dist / 5.5;
	if ( _slack < 20 ) then { _slack = 20 };

	_min_dist = floor(_max_dist / _wp_count) * 2;
	If (_min_dist > (_max_dist / 3)) then {_min_dist = floor(_max_dist / 3);};

	_angle_offset = random 360;
	DEBUG_1("Creating WPs",ARR_5(_grpNumber,_grp, _wp_count, _min_dist,_is_vec));
	_slope_dist = if (_is_vec) then { 5 } else { 1 };
	while {count _wp_array < _wp_count && !isNull _grp && {!isNull (leader _grp)}} do 
	{
		private ["_x1","_y1","_wp_pos", "_prepos","_bldgpos","_bldgs","_not_found","_i","_too_close","_roads","_road","_bld","_pos_details"];
		_wp_pos = [];
		_newangle = (count _wp_array * _angle) + _angle_offset;

		_x1 = _center_x - (sin _newangle * _max_dist);
		_y1 = _center_y - (cos _newangle * _max_dist);

		_prepos = [_x1, _y1, _center_z];
		if ( isNil "_center_z" ) then {
			_prepos = [_x1, _y1];
		};
		
		// _wp_pos = [_prepos, 0, _slack, 6, 0, 50 * (pi / 180), 0, _blacklist] call BIS_fnc_findSafePos;
		// We give 30 tries to find a suitable point, then we pick the first one available.
		_not_found = 30;
		_min_wp_dist = 0;
		_previous_wp = [];
		while {_not_found > 0 && !isNull _grp && !isNull (leader _grp)} do {
			_not_found = _not_found - 1;
			// We look for a random point in the circle.
			_wp_pos = [_pos,_max_dist,((_max_dist / 5) min 10),_slope_dist] call FFUNC(getRanPointCircle);
			// in the particular case of vehicle groups, we look for roads.
			if (_is_vec && ((count _wp_pos) > 1)) then {
				_dist = (_max_dist / 4);
				_roads = _wp_pos nearRoads _dist;
				if (count _roads > 0) then {
					_road = _roads select 0;
					{
						if ((_x distance _wp_pos) < _dist) then {
							_dist = (_x distance _wp_pos);
							_road = _x;
						};
					} foreach _roads;
					_wp_pos = getPos _road;
					TRACE_1("Road",ARR_4(_grpNumber,_grp,_wp_pos, _not_found));
				};
			};
			// Next we'll check it's not too close to the other points (to ensure we run around a large part of the circle)
			if ((count _wp_pos) > 1) then {
				_too_close = False;
				If ((count _wp_array) > 1) then {
					_previous_wp = _wp_array select ((count _wp_array) - 1);
				} else {
					_previous_wp = getPos (leader _grp);
				};
				TRACE_1("Previous WP",ARR_5(_grpNumber,_grp, _previous_wp,(count _wp_array),(leader _grp)));
				_min_wp_dist = _wp_pos distance _previous_wp;
				if (_min_wp_dist < _min_dist) then {
					TRACE_1("Too close",ARR_2(_min_wp_dist,_min_dist));
					_too_close = true;
				};
				if (!_too_close) then { _not_found = 0; };
			};
		};
		TRACE_1("New WP",ARR_5(_grpNumber,_grp, _wp_pos,_min_wp_dist, _is_vec));
		if (!isNull _grp && {!isNull (leader _grp)} && {count _wp_pos == 3}) then{
			if (!_is_vec && {random(10) > 6}) then {
				// Add building search
				if ((count _wp_pos > 0) And (leader _grp isKindOf "Man")) then {
					_bldgpos = [];
					_bldgs = _wp_pos nearObjects ["Building", 30];
					TRACE_1("Found Buildings",ARR_4(_grpNumber,_grp, _bldgs, _is_vec));
					{
						_bld = _x;
						_bldarr = [_bld,TMF_BUILDING_POS_COMBAT,_grpNumber] call FFUNC(buildingPosArray);
						If (count _bldarr > 0) then {
							{ _bldgpos set [count _bldgpos,_x];	
							} foreach _bldarr;
						};
					} forEach _bldgs;
					if(count _bldgpos != 0) then {
						_pos_details = _bldgpos call FFUNC(arrayRandomVal);
						_wp_pos = _pos_details select 0;
						TRACE_1("Building pos",ARR_4(_grpNumber,_grp, _pos_details, count(_bldgpos)));
					};
					_wp_array = _wp_array + [_wp_pos];
				};
			} else {
				_wp_array = _wp_array + [_wp_pos];
			};
			sleep 0.5;
		};
	};

	sleep 1;
	if (!isNull _grp && !isNull (leader _grp) && _wp_count >= 1 && (count _wp_array) >= _wp_count) then {
		[_grp] call FFUNC(deleteWaypoints);
		DEBUG_1("Adding WPs to group",ARR_3(_grpNumber,_grp, _wp_count));
		_sad = 7.5;
		for "_i" from 0 to ((count _wp_array) - 1) do
		{
			private ["_wp","_cur_pos"];

			_cur_pos = (_wp_array select _i);

			// Create waypoints based on array of positions
			_wp = _grp addWaypoint [_cur_pos, 30];
			// _wp setWaypointCompletionRadius (5 + _slack);
			_wp setWaypointCompletionRadius _max_dist;
			_wp setWaypointType "MOVE";
			_wp setWaypointSpeed "LIMITED";
			_wp setWaypointTimeout [5, 9, 12];
			// 20% chance that we'll search the surroundings
			// But we never take the first point as SAD since the last always is SAD
			if ((_i > 0) && ((random 10) >= _sad)) then {
				if (!_is_vec) then {
					_wp setWaypointType "SAD";
					_wp setWaypointTimeout [1, 3, 7];
				} else {
					_wp setWaypointTimeout [5, 10, 20];
				};
				_sad = _sad + 0.5;
			};
			//_wp setWaypointTimeout [0, 2, 16];
			// When completing waypoint have 33% chance to choose a random next wp. The calculation at the end takes a random waypoint with the first WP not included!
			// _wp setWaypointStatements ["true", "if ((random 3) > 2) then { _nextWP = ((floor (random ((count (waypoints (group this)))-1))) + 1); diag_log Format['TaskPatrol [%1]: New WP %2 (%3)',(group this), _nextWP, (count (waypoints (group this)))]; group this setCurrentWaypoint [(group this), _nextWP];};"];
			_wp setWaypointStatements ["true", "if ((random 3) > 2) then { _nextWP = ((floor (random ((count (waypoints (group this)))-1))) + 1); group this setCurrentWaypoint [(group this), _nextWP];};"];
			// 30% chance that the formation type will change.
			if (_is_vec) then {
				_wp setWaypointFormation _formation;
			} else {
				If (floor(random 10) >= 7) then {
					_wp setWaypointFormation (["STAG COLUMN", "WEDGE", "ECH LEFT", "ECH RIGHT", "VEE", "DIAMOND"] call FFUNC(arrayRandomVal));
				};
			};
			TRACE_1("Added",ARR_6(_grpNumber,_grp,_i, _cur_pos,waypointType _wp,waypointFormation _wp));
			sleep 0.5;
		};
		// End back near start point and then pick a new random point
		_wp1 = _grp addWaypoint [_pos, 10];
		if (!_is_vec) then {
			_wp1 setWaypointType "SAD";
		} else {
			_wp1 setWaypointType "MOVE";
		};
		_wp1 setWaypointTimeout [10, 15, 25];
		_wp1 setWaypointCompletionRadius _max_dist;
		[_grp,(count waypoints _grp)] setWaypointStatements ["true", "group this setCurrentWaypoint [(group this), (round (random 2) + 1)];"];
		// We reinitialize the formation to the initial one.
		_wp1 setWaypointFormation _formation;

		// Cycle in case we reach the end
		_wp2 = _grp addWaypoint [(_wp_array select 0), 10];
		_wp2 setWaypointType "CYCLE";
		_wp2 setWaypointCompletionRadius _min_dist;

		_grp setCurrentWaypoint [_grp, 0];
	};

	DEBUG_1("Group Initialized with waypoints",ARR_3(_grpNumber,_grp,(count waypoints _grp)));
	_grp setVariable ["patrol",_pos];

	if (IS_TRACE && !isNull _grp && !isNull (leader _grp)) then {
		_marker_pos = getPos (leader _grp);
		_marker_name = Format["%1%2",str(_grp),0];
		_marker = createMarkerLocal[_marker_name,[_marker_pos select 0,_marker_pos select 1]];
		_marker setMarkerShapeLocal "ICON";
		_marker_name setMarkerTypeLocal "mil_dot";
		_marker_name setMarkerColorLocal "ColorRed";
		_marker_name setMarkerTextLocal Format["%1",_grpNumber];
		_marker_name setMarkerAlphaLocal 0.5;
		if ((count waypoints _grp) > 0) then {
			_i = 1;
			private ["_marker","_marker_name","_marker_pos"];
			{
				_marker_pos = waypointPosition _x;
				_marker_name = Format["%1%2",str(_marker_pos),_i];
				_marker = createMarkerLocal[_marker_name,[_marker_pos select 0,_marker_pos select 1]];
				_marker setMarkerShapeLocal "ICON";
				_marker_name setMarkerTypeLocal "mil_triangle";
				_marker_name setMarkerColorLocal "ColorGreen";
				_marker_name setMarkerTextLocal Format["P-%1-%2",_grpNumber, str(_i)];
				_marker_name setMarkerAlphaLocal 0.5;
				TRACE_1("WP,Type,Form,Pos,",ARR_6(_grpNumber,_grp,(currentWaypoint _grp),waypointType _x,waypointFormation _x,waypointPosition _x));
				_i = _i + 1;
			} foreach waypoints _grp;
		};
	};
} else {
	ERROR("Group is not set");
};

TRACE("End");
TMFPROFILERSTOP;